/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file    can.c
  * @brief   This file provides code for the configuration
  *          of the CAN instances.
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2025 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "can.h"

/* USER CODE BEGIN 0 */
#include "xiaomi.h"


void Filter_Init(void)
{
    // CAN1 filter config(use bank 0)
    CAN_FilterTypeDef sFilterCfg;  
    sFilterCfg.FilterActivation = CAN_FILTER_ENABLE; //enable filter
    // 0~13 belong to CAN1,use Bank 0
    sFilterCfg.FilterBank       = 0;                   //Filter group 0,
    sFilterCfg.FilterMode       = CAN_FILTERMODE_IDMASK; //Operating mode
    sFilterCfg.FilterScale      = CAN_FILTERSCALE_32BIT; //Filter bit width bit
    sFilterCfg.FilterFIFOAssignment = CAN_RX_FIFO0;   //Filter association FIFO setting: 0
    // The ID & Mask are both set to 0,full acceptance
    sFilterCfg.FilterMaskIdHigh = 0x0000;  
    sFilterCfg.FilterMaskIdLow  = 0x0000;
    sFilterCfg.FilterIdHigh     = 0x0000;   
    sFilterCfg.FilterIdLow      = 0x0000;    
    sFilterCfg.SlaveStartFilterBank = 14;    //Set the filter group. For a single CAN instance, this parameter has no significance.
    HAL_CAN_ConfigFilter(&hcan1,&sFilterCfg); 
    
    // CAN2 filter config(use bank 14)
    sFilterCfg.FilterBank       = 14;        //Filter Group 14, Note: 14 and above belong to CAN2.           
    sFilterCfg.FilterIdHigh     = 0x0000;
    sFilterCfg.FilterIdLow      = 0x0000;
    sFilterCfg.FilterMaskIdHigh = 0x0000;
    sFilterCfg.FilterMaskIdLow  = 0x0000;
    sFilterCfg.FilterMode       = CAN_FILTERMODE_IDMASK; 
    sFilterCfg.FilterScale      = CAN_FILTERSCALE_32BIT;
    sFilterCfg.FilterFIFOAssignment = CAN_RX_FIFO0;  
    sFilterCfg.FilterActivation = CAN_FILTER_ENABLE;
    sFilterCfg.SlaveStartFilterBank = 14;  
    HAL_CAN_ConfigFilter(&hcan2,&sFilterCfg);
}

//Interrupt callback function
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
    uint8_t rxData[8];
    CAN_RxHeaderTypeDef rxHeader;  
    //Determine the data is sent from the instance object of CAN1
    if(hcan->Instance == CAN1)
    {
        HAL_CAN_GetRxMessage(hcan,CAN_FilterFIFO0,&rxHeader,rxData);

        uint32_t extId = rxHeader.ExtId; 
        uint8_t commType = (extId >> 24) & 0x1F; 
        uint8_t motorId  = (extId >> 8) & 0xFF;  
        if(commType == COMM_TYPE_READ_PARAM && motorId == MOTOR_ID)
        {
            float val;
            memcpy(&val, &rxData[4], 4);
            g_Motor1CurrentRadian = val;
            g_Motor1ReadDone = true;

        }
    }
	//Determine the data is sent from the instance object of CAN2
    if(hcan->Instance == CAN2)
    {
        HAL_CAN_GetRxMessage(hcan,CAN_FilterFIFO0,&rxHeader,rxData);
        uint32_t extId = rxHeader.ExtId; 
        uint8_t commType = (extId >> 24) & 0x1F; 
        uint8_t motorId  = (extId >> 8) & 0xFF;  
        if(commType == COMM_TYPE_READ_PARAM && motorId == MOTOR_ID)
        {
            float val;
            memcpy(&val, &rxData[4], 4);
			g_Motor2CurrentRadian = val;
            g_Motor2ReadDone = true;
        }
    }
}
/* USER CODE END 0 */

CAN_HandleTypeDef hcan1;
CAN_HandleTypeDef hcan2;

/* CAN1 init function */
void MX_CAN1_Init(void)
{

  /* USER CODE BEGIN CAN1_Init 0 */

  /* USER CODE END CAN1_Init 0 */

  /* USER CODE BEGIN CAN1_Init 1 */

  /* USER CODE END CAN1_Init 1 */
  hcan1.Instance = CAN1;
  hcan1.Init.Prescaler = 3;
  hcan1.Init.Mode = CAN_MODE_NORMAL;
  hcan1.Init.SyncJumpWidth = CAN_SJW_1TQ;
  hcan1.Init.TimeSeg1 = CAN_BS1_11TQ;
  hcan1.Init.TimeSeg2 = CAN_BS2_2TQ;
  hcan1.Init.TimeTriggeredMode = DISABLE;
  hcan1.Init.AutoBusOff = DISABLE;
  hcan1.Init.AutoWakeUp = DISABLE;
  hcan1.Init.AutoRetransmission = DISABLE;
  hcan1.Init.ReceiveFifoLocked = DISABLE;
  hcan1.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan1) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN1_Init 2 */

  /* USER CODE END CAN1_Init 2 */

}
/* CAN2 init function */
void MX_CAN2_Init(void)
{

  /* USER CODE BEGIN CAN2_Init 0 */

  /* USER CODE END CAN2_Init 0 */

  /* USER CODE BEGIN CAN2_Init 1 */

  /* USER CODE END CAN2_Init 1 */
  hcan2.Instance = CAN2;
  hcan2.Init.Prescaler = 3;
  hcan2.Init.Mode = CAN_MODE_NORMAL;
  hcan2.Init.SyncJumpWidth = CAN_SJW_1TQ;
  hcan2.Init.TimeSeg1 = CAN_BS1_11TQ;
  hcan2.Init.TimeSeg2 = CAN_BS2_2TQ;
  hcan2.Init.TimeTriggeredMode = DISABLE;
  hcan2.Init.AutoBusOff = DISABLE;
  hcan2.Init.AutoWakeUp = DISABLE;
  hcan2.Init.AutoRetransmission = DISABLE;
  hcan2.Init.ReceiveFifoLocked = DISABLE;
  hcan2.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan2) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN2_Init 2 */

  /* USER CODE END CAN2_Init 2 */

}

static uint32_t HAL_RCC_CAN1_CLK_ENABLED=0;

void HAL_CAN_MspInit(CAN_HandleTypeDef* canHandle)
{

  GPIO_InitTypeDef GPIO_InitStruct = {0};
  if(canHandle->Instance==CAN1)
  {
  /* USER CODE BEGIN CAN1_MspInit 0 */

  /* USER CODE END CAN1_MspInit 0 */
    /* CAN1 clock enable */
    HAL_RCC_CAN1_CLK_ENABLED++;
    if(HAL_RCC_CAN1_CLK_ENABLED==1){
      __HAL_RCC_CAN1_CLK_ENABLE();
    }

    __HAL_RCC_GPIOD_CLK_ENABLE();
    /**CAN1 GPIO Configuration
    PD0     ------> CAN1_RX
    PD1     ------> CAN1_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_0|GPIO_PIN_1;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF9_CAN1;
    HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);

    /* CAN1 interrupt Init */
    HAL_NVIC_SetPriority(CAN1_RX0_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN1_RX0_IRQn);
    HAL_NVIC_SetPriority(CAN1_RX1_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn);
  /* USER CODE BEGIN CAN1_MspInit 1 */

  /* USER CODE END CAN1_MspInit 1 */
  }
  else if(canHandle->Instance==CAN2)
  {
  /* USER CODE BEGIN CAN2_MspInit 0 */

  /* USER CODE END CAN2_MspInit 0 */
    /* CAN2 clock enable */
    __HAL_RCC_CAN2_CLK_ENABLE();
    HAL_RCC_CAN1_CLK_ENABLED++;
    if(HAL_RCC_CAN1_CLK_ENABLED==1){
      __HAL_RCC_CAN1_CLK_ENABLE();
    }

    __HAL_RCC_GPIOB_CLK_ENABLE();
    /**CAN2 GPIO Configuration
    PB12     ------> CAN2_RX
    PB13     ------> CAN2_TX
    */
    GPIO_InitStruct.Pin = GPIO_PIN_12|GPIO_PIN_13;
    GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
    GPIO_InitStruct.Pull = GPIO_NOPULL;
    GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
    GPIO_InitStruct.Alternate = GPIO_AF9_CAN2;
    HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);

    /* CAN2 interrupt Init */
    HAL_NVIC_SetPriority(CAN2_RX0_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN2_RX0_IRQn);
    HAL_NVIC_SetPriority(CAN2_RX1_IRQn, 0, 0);
    HAL_NVIC_EnableIRQ(CAN2_RX1_IRQn);
  /* USER CODE BEGIN CAN2_MspInit 1 */

  /* USER CODE END CAN2_MspInit 1 */
  }
}

void HAL_CAN_MspDeInit(CAN_HandleTypeDef* canHandle)
{

  if(canHandle->Instance==CAN1)
  {
  /* USER CODE BEGIN CAN1_MspDeInit 0 */

  /* USER CODE END CAN1_MspDeInit 0 */
    /* Peripheral clock disable */
    HAL_RCC_CAN1_CLK_ENABLED--;
    if(HAL_RCC_CAN1_CLK_ENABLED==0){
      __HAL_RCC_CAN1_CLK_DISABLE();
    }

    /**CAN1 GPIO Configuration
    PD0     ------> CAN1_RX
    PD1     ------> CAN1_TX
    */
    HAL_GPIO_DeInit(GPIOD, GPIO_PIN_0|GPIO_PIN_1);

    /* CAN1 interrupt Deinit */
    HAL_NVIC_DisableIRQ(CAN1_RX0_IRQn);
    HAL_NVIC_DisableIRQ(CAN1_RX1_IRQn);
  /* USER CODE BEGIN CAN1_MspDeInit 1 */

  /* USER CODE END CAN1_MspDeInit 1 */
  }
  else if(canHandle->Instance==CAN2)
  {
  /* USER CODE BEGIN CAN2_MspDeInit 0 */

  /* USER CODE END CAN2_MspDeInit 0 */
    /* Peripheral clock disable */
    __HAL_RCC_CAN2_CLK_DISABLE();
    HAL_RCC_CAN1_CLK_ENABLED--;
    if(HAL_RCC_CAN1_CLK_ENABLED==0){
      __HAL_RCC_CAN1_CLK_DISABLE();
    }

    /**CAN2 GPIO Configuration
    PB12     ------> CAN2_RX
    PB13     ------> CAN2_TX
    */
    HAL_GPIO_DeInit(GPIOB, GPIO_PIN_12|GPIO_PIN_13);

    /* CAN2 interrupt Deinit */
    HAL_NVIC_DisableIRQ(CAN2_RX0_IRQn);
    HAL_NVIC_DisableIRQ(CAN2_RX1_IRQn);
  /* USER CODE BEGIN CAN2_MspDeInit 1 */

  /* USER CODE END CAN2_MspDeInit 1 */
  }
}

/* USER CODE BEGIN 1 */

/**
 * The Xiaomi servo motor sends CAN frame data for encapsulation
**/
void CAN_SendFrame(CAN_HandleTypeDef *hcan,can_frame_t* frame) {
    CAN_TxHeaderTypeDef TxHeader;  //Send configuration struct
    uint8_t TxData[8];              //Send data initialize
    uint32_t TxMailbox = CAN_TX_MAILBOX0; //Mail
    uint32_t extended_id ;             //extended ID
	//According to the Xiaomi instruction manual, for the 29-bit ID: bits 28 to 24 represent the communication type, bits 23 to 8 represent the data, and bits 7 to 0 represent the target motor (CAN-ID).
    extended_id = (frame->mode) << 24 | (frame->data)<<8 | (frame->id);

    TxHeader.ExtId = extended_id;//setting extended ID
    TxHeader.IDE = CAN_ID_EXT;
    TxHeader.RTR = CAN_RTR_DATA;
    TxHeader.DLC = 8;

    // copy the data,copy the data from the 8Byte data area to TxData
    memcpy(TxData, frame->tx_data, 8);
    // send data
    if (HAL_CAN_AddTxMessage(hcan, &TxHeader, TxData, &TxMailbox) != HAL_OK)
	{
		printf("CAN send data fail!!!");
	}
}




/* USER CODE END 1 */
